#include "nav2_costmap_2d/voronoi_layer.hpp"
#include <chrono>  // NOLINT
#include <vector>
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::VoronoiLayer, nav2_costmap_2d::Layer)

namespace nav2_costmap_2d
{
void VoronoiLayer::onInitialize() {
    current_ = true;
}
 
void VoronoiLayer::updateBounds(double /*robot_x*/, double /*robot_y*/, double /*robot_yaw*/,
                                double* /*min_x*/, double* /*min_y*/, double* /*max_x*/,
                                double* /*max_y*/) { }

bool VoronoiLayer::OutlineMap(const nav2_costmap_2d::Costmap2D& master_grid,
                              uint8_t value) {  
    uint8_t* char_map = master_grid.getCharMap();
    unsigned int size_x = master_grid.getSizeInCellsX();
    unsigned int size_y = master_grid.getSizeInCellsY();
    if (char_map == nullptr) { 
        //LOG(ERROR) << "char_map == nullptr";
        return false;
    }

    uint8_t* pc = char_map;
    for (unsigned int i = 0U; i < size_x; ++i) {
        *pc++ = value;
    }
    pc = char_map + (size_y - 1U) * size_x;
    for (unsigned int i = 0U; i < size_x; ++i) {
        *pc++ = value;
    }
    pc = char_map;
    for (unsigned int i = 0U; i < size_y; ++i, pc += size_x) {
        *pc = value;
    }
    pc = char_map + size_x - 1U;
    for (unsigned int i = 0U; i < size_y; ++i, pc += size_x) {
        *pc = value;
    }
    return true;
}

void VoronoiLayer::reset() {
    // TODO(ljr) : 注释掉了下行代码 
    // voronoi_.initializeMaps();
}

void VoronoiLayer::UpdateDynamicVoronoi(const nav2_costmap_2d::Costmap2D& master_grid) {
    unsigned int size_x = master_grid.getSizeInCellsX();
    unsigned int size_y = master_grid.getSizeInCellsY();

    voronoi_.initializeEmpty(static_cast<int>(size_x),
                             static_cast<int>(size_y));

    if (last_size_x_ != size_x || last_size_y_ != size_y) {
        voronoi_.initializeEmpty(static_cast<int>(size_x),
                                 static_cast<int>(size_y));
        last_size_x_ = size_x;
        last_size_y_ = size_y;
    }
    std::vector<IntPoint> new_free_cells;
    std::vector<IntPoint> new_occupied_cells;
    for (int j = 0; j < static_cast<int>(size_y); ++j) {
        for (int i = 0; i < static_cast<int>(size_x); ++i) {
        if (voronoi_.isOccupied(i, j) &&
            master_grid.getCost(i, j) == nav2_costmap_2d::FREE_SPACE) {
            new_free_cells.emplace_back(i, j);
        }

        if (!voronoi_.isOccupied(i, j) &&
          (master_grid.getCost(i, j) == nav2_costmap_2d::LETHAL_OBSTACLE || 
           master_grid.getCost(i, j) == nav2_costmap_2d::NO_INFORMATION)) {
            //std::cout<<"\033[31mnew occupied cells\033[0m"<<std::endl;
            new_occupied_cells.emplace_back(i, j);
      }
        }
    }

    for (const IntPoint& cell : new_free_cells) {
        voronoi_.clearCell(cell.x, cell.y);
    }

    for (const IntPoint& cell : new_occupied_cells) {
        voronoi_.occupyCell(cell.x, cell.y);
    }
    voronoi_.exchangeObstacles(new_occupied_cells);//用新的障碍物更新地图，传入新的障碍物的位置信息(newObstacles)。

    voronoi_.update(true);
    voronoi_.prune();
    // voronoi_.updateAlternativePrunedDiagram();
    // End timing.
}

void VoronoiLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int /*min_i*/, int /*min_j*/,
                               int /*max_i*/, int /*max_j*/) {
    //if (!enabled_) {
    if(!update_enabled_){
        return;
    }
    update_enabled_ = false;
    if (false) {
        //LOG(ERROR) << "VoronoiLayer is disable.";
        std::cout << "VoronoiLayer is disable." << std::endl;
        return;
    }

    std::lock_guard<std::mutex> lock(mutex_);

    if (!OutlineMap(master_grid, nav2_costmap_2d::LETHAL_OBSTACLE)) {
        //LOG(ERROR) << "Failed to outline map.";
        std::cout << "Failed to outline map." << std::endl;
        return;
    }

    // Start timing.
    const auto start_timestamp = std::chrono::system_clock::now();
    // voronoi_.removeObstacle(master_grid.x, cell.y);
    UpdateDynamicVoronoi(master_grid);
    voronoi_.update(true);// 再次对地图进行更新，以反映新的障碍物信息。

    voronoi_.prune();//再次对地图进行修剪。
    if(visualize_counter_ == 0){
        voronoi_.visualize("/home/bigdavid/Lecture_4_2/src/simulation/maps/map.pgm");
        visualize_counter_++;
    }


    const auto end_timestamp = std::chrono::system_clock::now();
    const std::chrono::duration<double> diff = end_timestamp - start_timestamp;
    //VLOG(4) << std::fixed << "Runtime=" << diff.count() * 1e3 << "ms.";
    // return;
}

}